#include "H/Function_Init.H"

bit claw_en, claw_interrupt_flag;
uint8_t claw_step, claw_tm;

void Claw_Start(void)
{
	if(claw_en)return;
	
	claw_en = 1;
	claw_step = 0;
	claw_tm = 0;
}

void ClawM_Ctrl(void)
{
	if(!claw_en)return;
	if(!claw_interrupt_flag)return;		// 100ms
	claw_interrupt_flag = 0;
	
	switch(claw_step)
	{
		case 0:{
			Motor_1_ctrl(1);
			claw_step = 1;
			claw_tm = 30;
		}break;
		
		case 1:{
			if(claw_tm)claw_tm--;
			else{
				Motor_1_ctrl(0);
				claw_step = 2;
				claw_tm = 20;
			}
		}break;
		
		case 2:{
			if(claw_tm)claw_tm--;
			else{
				Motor_1_ctrl(2);
				claw_step = 3;
				claw_tm = 30;
			}
		}break;
		
		case 3:{
			if(claw_tm)claw_tm--;
			else{
				Motor_1_ctrl(0);
				claw_step = 0;
				claw_tm = 0;
				claw_en = 0;
			}
		}break;
		
		default:break;
	}
}


